package edu.mapi.ir.controllers;

import edu.mapi.ir.state.EstimatedState;

/**
 * @author ZP
 */
public class WalkController extends AbstractController {

	protected double distance2walk;
	protected double maxPower = 0.1;
	
	public WalkController(double distance) {
		this.distance2walk = distance;
	}
	
	public Actuation decide(Actuation lastActuation, EstimatedState state) {
		
		double decLeft = 0, decRight = 0;
		
		if (state.distanceSensors[1] > 1)
			decRight = maxPower /3;
		
		if (state.distanceSensors[2] > 1)
			decLeft = maxPower /3;
			
		if (distance2walk < (state.getLOutPow()+maxPower)/2) {
			double finalDist = distance2walk;
    		double finalPower = 2*finalDist - lastActuation.leftPower;
    		distance2walk -= (state.getLOutPow()+finalPower)/2;
    		setComplete(true);
    		return new Actuation(finalPower, finalPower);    		
		}
		
		double pout = (state.getLOutPow()+maxPower)/2;
		distance2walk -= pout;
		return new Actuation(maxPower-decLeft, maxPower-decRight);
	}
	
}
